#include "MotorController.hpp"
#include "system.h"

#include "stdio.h"

#include "apm32a10x_rcm.h"
#include "apm32a10x_gpio.h"
#include "apm32a10x_tmr.h"

static const double __pid_kp = 3.0;
static const double __pid_ki = 0.0135;
static const double __pid_kd = 0.2;

static const uint16_t __timer_period = 999;
static double __force = 0.0;
static double dead_zone = 0.01;
static double allowable_error_limit = 0.005;

extern bool inc_data_sync;
static const struct {
    /* Motor driver. */
    GPIO_T *port_out1;
    GPIO_PIN_T pin_out1;
    PeripheralClock_t port_clk_out1;
    GPIO_T *port_out2;
    GPIO_PIN_T pin_out2;
    PeripheralClock_t port_clk_out2;
    TMR_T *tmr_out;
    PeripheralClock_t tmr_clk_out;
    TMR_CHANNEL_T tmr_ch_out;

    /* Position sensor. */
    ADC_T *adc_position_sensor;
    PeripheralClock_t adc_clk_position_sensor;

    /* SPI interface. */
}__hardware_description = {
    .port_out1 = GPIOC,
    .pin_out1 = GPIO_PIN_7,
    .port_clk_out1 = CLK_GPIOC,
    .port_out2 = GPIOB,
    .pin_out2 = GPIO_PIN_0,
    .port_clk_out2 = CLK_GPIOB,
    .tmr_out = TMR8,
    .tmr_clk_out = CLK_TMR8,
    .tmr_ch_out = TMR_CHANNEL_2,

    .adc_position_sensor = ADC1,
    .adc_clk_position_sensor = CLK_ADC1
};

MotorController::MotorController(ADCUtil *adc_util){
    this->adc_util = adc_util;
    this->position_setpoint = system_configuration.motor_position;
    if ( this->position_setpoint < 0.0 ) this->position_setpoint = 0.0;
    if ( this->position_setpoint > 1.0 ) this->position_setpoint = 1.0;
    this->motor_motion_margin = system_configuration.motor_motion_margin;
    if ( this->motor_motion_margin < 0.0 ) this->motor_motion_margin = 0.0;
    if ( this->motor_motion_margin > 1.0 ) this->motor_motion_margin = 1.0;
}

MotorController::~MotorController(){
    if ( this->pli != nullptr ){
        delete this->pli;
        this->pli = nullptr;
    }
    if ( this->pid_controller != nullptr ){
        delete this->pid_controller;
        this->pid_controller = nullptr;
    }
}

bool MotorController::init(void){
    
    /* Enable peripheral clock. */

    System_setPeripheralClockEnable(__hardware_description.port_clk_out1,TRUE);
    System_setPeripheralClockEnable(__hardware_description.port_clk_out2,TRUE);
    System_setPeripheralClockEnable(__hardware_description.adc_clk_position_sensor,TRUE);
    System_setPeripheralClockEnable(__hardware_description.tmr_clk_out,TRUE);

    /* Initialize PWM output. */

    GPIO_Config_T gpio_configuration = {0};
    /* Out1. */
    gpio_configuration.pin = __hardware_description.pin_out1;
    gpio_configuration.mode = GPIO_MODE_OUT_PP;
    gpio_configuration.speed = GPIO_SPEED_50MHz;
    GPIO_Config(__hardware_description.port_out1,&gpio_configuration);
    /* Out2. */
    gpio_configuration.pin = __hardware_description.pin_out2;
    GPIO_Config(__hardware_description.port_out2,&gpio_configuration);
    /* Timer8. */
    TMR_BaseConfig_T timer_base_configuration;
    timer_base_configuration.clockDivision = TMR_CLOCK_DIV_1;
    timer_base_configuration.division = 71;
    timer_base_configuration.period = __timer_period;
    timer_base_configuration.repetitionCounter = 0;
    timer_base_configuration.countMode = TMR_COUNTER_MODE_UP;
    TMR_ConfigTimeBase(__hardware_description.tmr_out,&timer_base_configuration);
    TMR_OCConfig_T timer_oc_configuration;
    timer_oc_configuration.mode = TMR_OC_MODE_PWM1;
    timer_oc_configuration.idleState = TMR_OC_IDLE_STATE_RESET;
    timer_oc_configuration.nIdleState = TMR_OC_NIDLE_STATE_RESET;
    timer_oc_configuration.polarity = TMR_OC_POLARITY_HIGH;
    timer_oc_configuration.nPolarity = TMR_OC_NPOLARITY_HIGH;
    timer_oc_configuration.outputState = TMR_OC_STATE_ENABLE;
    timer_oc_configuration.outputNState = TMR_OC_NSTATE_ENABLE;
    timer_oc_configuration.pulse = 0;
    switch ( __hardware_description.tmr_ch_out ){
        case TMR_CHANNEL_1: TMR_ConfigOC1(__hardware_description.tmr_out,&timer_oc_configuration); break;
        case TMR_CHANNEL_2: TMR_ConfigOC2(__hardware_description.tmr_out,&timer_oc_configuration); break;
        case TMR_CHANNEL_3: TMR_ConfigOC3(__hardware_description.tmr_out,&timer_oc_configuration); break;
        case TMR_CHANNEL_4: TMR_ConfigOC4(__hardware_description.tmr_out,&timer_oc_configuration); break;
    }
    TMR_Enable(__hardware_description.tmr_out);
    TMR_EnablePWMOutputs(__hardware_description.tmr_out);


    /* Load piecewise linear interpolation datas. */
    this->pli = new PiecewiseLinearInterpolation();
    this->pli->load(system_motion_characteristic_data.x_data,system_motion_characteristic_data.y_data,system_motion_characteristic_data.n_data_points);
    /* Initialize PID controller. */
    this->pid_controller = new PIDController();
    this->pid_controller->setKp(__pid_kp);
    this->pid_controller->setKi(__pid_ki);
    this->pid_controller->setKd(__pid_kd);

    return true;
}

void MotorController::loop(void){
    // double error = this->getPosition() - this->position_setpoint;
    static double positoin = 0; 
    double pos = this->getPosition();
    double error = this->position_setpoint - pos;
    // printf("error %f,setpoint:%f,pos:%f\r\n",error,this->position_setpoint,pos);
    positoin = this->getPosition();
    
    /* 电机位置计算转换 仅读取*/
    if( system_configuration.position_switch == 1){
        system_configuration.motor_position = 1 - positoin;
    }else{
        system_configuration.motor_position = positoin;
    }

    if ( error < allowable_error_limit && error > -allowable_error_limit ) error = 0;

    /* PID controller iterate. */
    double force = this->pid_controller->iterate(error);
    this->motor_force = force;
    __force = force;
    this->setForce(force);
}

void MotorController::updatePli(){
    this->pli->load(system_motion_characteristic_data.x_data,system_motion_characteristic_data.y_data,system_motion_characteristic_data.n_data_points);
}

double MotorController::getSetPosition(){
    return position_setpoint;
}

void MotorController::setPosition(double pos){
    if ( pos < 0.0 ) pos = 0.0;
    if ( pos > 1.0 ) pos = 1.0; 
    
    this->position_setpoint = this->pli->calculate(pos);
    if (this->position_setpoint < system_configuration.motor_position_minimum) this->position_setpoint = system_configuration.motor_position_minimum;
    if (this->position_setpoint > system_configuration.motor_position_maximum) this->position_setpoint = system_configuration.motor_position_maximum;
    /* 电机位置计算转换 实际运行*/
    if( system_configuration.position_switch == 1){
        this->position_setpoint = 1 - this->position_setpoint;
    }

}


extern double min_adc,max_adc;
extern SystemConfiguration system_configuration;
extern MotionCharacteristicData system_motion_characteristic_data;
double MotorController::getPosition(void){
    volatile double position = 0;
    double adc_voltage = this->adc_util->getChannel(ADCUtil::Channel::Hall);

    /* 归一化 */
    position = (system_configuration.motor_minlength_voltage - adc_voltage) / (system_configuration.motor_minlength_voltage - system_configuration.motor_maxlength_voltage);
    if(position<0) position = 0;
    if(position>1) position = 1;
    return position;
}

double MotorController::getPositionSetPoint(void){
    return this->position_setpoint;
}

double MotorController::getForce(void){
    return this->motor_force;
}

void MotorController::setForce(double f){
    volatile uint16_t compare = 0;
    GPIO_Config_T gpio_configuration;
    gpio_configuration.mode = GPIO_MODE_AF_PP;
    gpio_configuration.speed = GPIO_SPEED_50MHz;
    GPIO_ResetBit(__hardware_description.port_out1,__hardware_description.pin_out1);
    GPIO_ResetBit(__hardware_description.port_out2,__hardware_description.pin_out2);
    if ( f >= 0.0 ){
        /* Forward rotation. Set pin to alternate function mode. */
        gpio_configuration.pin = __hardware_description.pin_out1;
        GPIO_Config(__hardware_description.port_out1,&gpio_configuration);
        gpio_configuration.pin = __hardware_description.pin_out2;
        gpio_configuration.mode = GPIO_MODE_OUT_PP;
        GPIO_Config(__hardware_description.port_out2,&gpio_configuration);

        if ( f > 1.0 ) f = 1.0;
        /* Dead zone. */
        if ( f < dead_zone ) f = 0;
        compare = (uint16_t)((__timer_period + 1) * f);
    }else{
        /* Reverse rotation. */
        gpio_configuration.pin = __hardware_description.pin_out2;
        GPIO_Config(__hardware_description.port_out2,&gpio_configuration);
        gpio_configuration.pin = __hardware_description.pin_out1;
        gpio_configuration.mode = GPIO_MODE_OUT_PP;
        GPIO_Config(__hardware_description.port_out1,&gpio_configuration);

        f = -f;
        if ( f > 1.0 ) f = 1.0;
        /* Dead zone. */
        if ( f < dead_zone ) f = 0;
        compare = (__timer_period - (uint16_t)((__timer_period  - 10) * f));

    }

    /* Calculate compare value. */
    switch ( __hardware_description.tmr_ch_out ){

        case TMR_CHANNEL_1: TMR_ConfigCompare1(__hardware_description.tmr_out,compare); break;
        case TMR_CHANNEL_2: TMR_ConfigCompare2(__hardware_description.tmr_out,compare); break;
        case TMR_CHANNEL_3: TMR_ConfigCompare3(__hardware_description.tmr_out,compare); break;
        case TMR_CHANNEL_4: TMR_ConfigCompare4(__hardware_description.tmr_out,compare); break;
    }
}

void MotorController::debugPrint(void){
    printf("adc_value:%f\r\n",this->adc_util->getChannel(ADCUtil::Channel::Hall));
    printf("set_position:%f\r\n",this->position_setpoint);
    // printf("system_configuration.motor_maxlength_voltage:%f\r\n",system_configuration.motor_maxlength_voltage);
    // printf("system_configuration.motor_minlength_voltage:%f\r\n",system_configuration.motor_minlength_voltage);
    printf("get_position:%f\r\n",this->getPosition());

    
    double error = this->position_setpoint - this->getPosition();
    if ( error < allowable_error_limit && error > -allowable_error_limit ) error = 0;
    double force = this->pid_controller->iterate(error);
    printf("error:%f\r\n",error);
    printf("force:%f\r\n",force);
    this->pid_controller->debugPrint();
    
    printf("\r\n");


    // printf("Motor output:F=%.2lf,p=%.2lf,x=%.2lf\r\n",__force,this->getPosition(),this->getPositionSetPoint());
    // this->pid_controller->debugPrint();
    // printf("TMR:CC=0x%.8x,CNT=0x%.8x\r\n",__hardware_description.tmr_out->CC2,__hardware_description.tmr_out->CNT);
    // printf("------------\r\n\r\n");
}